﻿using System;
using System.Collections;

namespace Xfs
{
    [XfsObjectSystem]
    class XfsAstarComponentXfsUpdateSystem : XfsUpdateSystem<XfsAstarComponent>
    {
        public override void Update(XfsAstarComponent self)
        {
                
        }
    }

    public static class XfsAstarComponentHelper
    {
        #region Vector3Paths
        public static void FindPath(this XfsAstarComponent self, Vector3 target)
        {
            if (self.start == null || self.grids == null) return ;           
            
            self.Target = target;

            self.goal = XfsGridMapComponent.Insatnce.ToGrid(self.Target);

            if (self.goal == self.LastGoal) return;

            Console.WriteLine(XfsTimeHelper.CurrentTime() + " " + self.GetType().Name + " 27. target: " + self.Target.ToString());

            self.paths = self.FindPath(self.start, self.goal, self.grids);

            self.Vector3Path = XfsGridMapComponent.Insatnce.ToVector3List(self.paths);

            self.LastGoal = self.goal;

            Console.WriteLine(XfsTimeHelper.CurrentTime() + " " + self.GetType().Name + " 28. paths: " + self.paths.Count);
        }
        #endregion

        #region ArrayList
        //private static XfsPriorityQueue? openList = null;
        //private static XfsPriorityQueue? closedList = null;
        public static ArrayList FindPath(this XfsAstarComponent self, XfsGrid start, XfsGrid goal, XfsGrid[,] grids)
        {
            if (grids[start.x, start.z].Obstacle || grids[goal.x, goal.z].Obstacle) return new ArrayList();

            self.openList = XfsComponentFactory.Create<XfsPriorityQueue>();
            self.closedList = XfsComponentFactory.Create<XfsPriorityQueue>();
            self.openList?.Clear();
            self.closedList?.Clear();

            self.openList?.Add(start);
            start.G = 0.0;
            start.H = Math.Abs(goal.x - start.x) + Math.Abs(goal.z - start.z) + start.bH;
            start.F = start.G + start.H;
            XfsGrid? grid = null;
            while (self.openList?.Length != 0)
            {
                grid = self.openList?.First();
                if (grid == null) return new ArrayList();
                if (grid.x == goal.x && grid.z == goal.z)
                {
                    return self.CalculatePath(grid);
                }
                ArrayList neighbours = self.GetNeighbours(grid, grids);
                for (int i = 0; i < neighbours.Count; i++)
                {
                    XfsGrid? neighGrid = (XfsGrid)neighbours[i];
                    if (!self.closedList.Contains(neighGrid))
                    {
                        double costG = self.GetCostG(neighGrid, grid);
                        double costH = self.GetCostH(neighGrid, goal);
                        neighGrid.G = grid.G + costG;
                        neighGrid.H = costH;
                        neighGrid.F = neighGrid.G + neighGrid.H;
                        neighGrid.parentGrid = grid;
                        if (!self.openList.Contains(neighGrid))
                        {
                            self.openList.Add(neighGrid);
                        }
                    }
                }
                self.closedList?.Add(grid);
                self.openList?.Remove(grid);
            }
            if (grid.x != goal.x && grid.z != goal.z)
            {
                Console.WriteLine("Goal Not Find.");
                return new ArrayList();
            }
            return self.CalculatePath(grid);
        }
        private static ArrayList CalculatePath(this XfsAstarComponent self, XfsGrid? goal)
        {
            ArrayList list = new ArrayList();
            while (goal != null)
            {
                list.Add(goal);
                goal = goal.parentGrid;
            }
            list.Reverse();
            return list;
        }
        private static double GetCostG(this XfsAstarComponent self, XfsGrid neighour, XfsGrid grid)
        {
            double cost = 1.0;
            if (grid.x != neighour.x && grid.z != neighour.z)
            {
                cost = 1.4;
            }
            return cost;
        }
        private static double GetCostH(this XfsAstarComponent self, XfsGrid neighour, XfsGrid goal)
        {
            double cost = 0.0;
            double xx = Math.Abs(goal.x - neighour.x);
            double yy = Math.Abs(goal.z - neighour.z);
            if (xx >= yy)
            {
                cost = xx - yy + yy * 1.4 + neighour.bH;
            }
            else
            {
                cost = yy - xx + xx * 1.4 + neighour.bH;
            }
            return cost;
        }
        private static ArrayList GetNeighbours(this XfsAstarComponent self, XfsGrid grid, XfsGrid[,] grids)
        {
            ArrayList list = new ArrayList();
            int row = grid.z;
            int column = grid.x;
            self.AssignNeighbour(row - 1, column - 1, list, grids);    //左上
            self.AssignNeighbour(row - 1, column, list, grids);        //左上
            self.AssignNeighbour(row - 1, column + 1, list, grids);    //左上
            self.AssignNeighbour(row, column - 1, list, grids);        //左上
            self.AssignNeighbour(row, column + 1, list, grids);        //左上
            self.AssignNeighbour(row + 1, column - 1, list, grids);    //左上
            self.AssignNeighbour(row + 1, column, list, grids);        //左上
            self.AssignNeighbour(row + 1, column + 1, list, grids);    //左上
            return list;
        }
        private static void AssignNeighbour(this XfsAstarComponent self, int row, int column, ArrayList neighbours, XfsGrid[,] grids)
        {
            if (row > -1 && column > -1 && row < grids.GetLength(0) && column < grids.GetLength(1))
            {
                XfsGrid grid = grids[row, column];
                if (!grid.Obstacle)
                {
                    neighbours.Add(grid);
                }
            }
        }
        #endregion

    }
}